#include "arduino_serial.h"
#include "mysockets.h"
#include "time.h"
#include <stdio.h>
#include <pthread.h>

int arduino_fd = 0;

void *f_arduino_reader(){
	char msg[255];
	int r;

	while(1){
		//continuously reads and displays messages from the arduino
		r = serialport_read_until(arduino_fd, msg, '\n'); 
		if(r != -1){
			fprintf(stderr,"Arduino msg: '%s'\n",msg);
		}
	}
}

//reads n bytes from fd into msg
//returns 0 on success, -1 on error
int read_n_bytes(int n, int fd, char *msg){

	int num_read;
	int r;

	num_read = 0;

	while(num_read < n){
		r=read(fd, msg+num_read, n);
		if(r<0){
			return -1;
		}
		num_read+=r;
	}
	msg[n] = '\0';

	return 0;

}

int main(int argc, char ** argv){

	char * hostname;	//holds the host's name
	int android_port, android_socket;	//port and socket numbers
	int x_robot, y_robot, x_camera, y_camera;
	char header_byte;
	int r;
	char c;
	int n;
	char msg[256]; //max msg length is pre-determined
	int tty_num;
	int android_fd = 0;
	char str_arduino_port[256];
	pthread_t pt_arduino_reader;

	//check for correct usage
	if(argc != 4){
		fprintf(stderr, "Usage: ./myserver host-name android-port arduino-ttyACM#\n");
		exit(1);
	}

	//get the utk netreg name for this pc
	hostname = argv[1];

	//set the root directory, port, and environment variables
	android_port = atoi(argv[2]);

	//the arduino shows up either on ttyACM0 or ttyACM1
	tty_num = atoi(argv[3]);
	sprintf(str_arduino_port,"/dev/ttyACM%d",tty_num);

	//create a socket for talking to the android device
	android_socket = serve_socket(hostname, android_port);

	//get a file descriptor to communicate with the arduino
	arduino_fd = serialport_init(str_arduino_port, 9600);
	if(arduino_fd==-1){
		fprintf(stderr,"Error: could not open arduino's serial port\n");	
		return -1;
	}

	//create a thread that just reads lines from the arduino
	/*
	r = pthread_create(&pt_arduino_reader,NULL,f_arduino_reader,NULL);
	if(r == -1){
		fprintf(stderr,"Error: pthread_create\n");
		return -1;
	}
	*/

	//continuous loop keeps trying to connect
	while(1){

		//create a connection
		fprintf(stderr,"Waiting for connection\n");
		android_fd = accept_connection(android_socket);

		fprintf(stderr, "Connection established with client\n");

		while(1){
			//read a character (which will tell us the msg length)
			r=read(android_fd, &c, 1);
			if(r<1){
				//error
				fprintf(stderr,"Lost connection\n");
				break;
			}

			//convert char to int
			n = (int)c;

			if(n>0){
				//read the n byte msg
				r = read_n_bytes(n, android_fd, msg);

				if(r != 0){
					//error
					fprintf(stderr,"Lost connection\n");
					break;
				}

				//msg is a csv of 4 numbers:
				//x_robot,y_robot,x_camera,y_camera
				//read each number as an int

				//fprintf(stderr,"msg is: %s\n",msg);

				r = sscanf(msg,"%d,%d,%d,%d", &x_robot, &y_robot, &x_camera, &y_camera);
				//fprintf(stderr,"Read %d numbers\n",r);
				if(r != 4){
					fprintf(stderr,"Error - invalid msg: %s\n",msg);
					//read another char and msg
					continue;
				}

				//send header byte, then the message:

				//robot_x
				header_byte = (char)1;
				do{
					r = serialport_writebyte(arduino_fd, header_byte);
				}while(r==-1);

				//x_robot = (x_robot/(255/40));
				do{
					r = serialport_writebyte(arduino_fd, (char)x_robot);
				}while(r==-1);

				//robot_y
				header_byte = (char)2;
				do{
					r = serialport_writebyte(arduino_fd, header_byte);
				}while(r==-1);

				//y_robot = (y_robot/(255/40) + 40);
				do{
					r = serialport_writebyte(arduino_fd, (char)y_robot);
				}while(r==-1);

				fprintf(stderr,"Wrote to arduino:\n"
						"   x_robot: %d\n"
						"   y_robot: %d\n"
						,x_robot
						,y_robot);

			}

		}
		fprintf(stderr, "\nConnection closed\n");
	}
}
